#ifndef PATROLLER_H_
#define PATROLLER_H_

#include <ros/ros.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <tf/transform_datatypes.h>
#include <iostream>
#include <fstream>

typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;

/**
 * Moves robot along the patrol path
 */
class Patroller {

public:
	/**
	 * Constructor- creates a new object to run the patrol
	 * @param waitAtGoal how long (seconds) should wait at each goal
	 * @param numLaps how many laps should run the patrol
	 * @param logfileName name of log file
	 * @param waitForServer how long (seconds) should wait for action server
	 */
	Patroller(const float& waitAtGoal, const int& numLaps,
			const std::string& logfileName, const double& waitForServer);

	/**
	 * Destructor
	 */
	virtual ~Patroller();

	/**
	 * Runs the robot along given patrol path
	 * @param patrolPath list of patrol path locations
	 * @param initCost cost of path from initial position to patrol's starting point
	 * @param segmentsCost cost of each patrol path segment
	 */
	void runPatrol(const std::vector<geometry_msgs::Point>& patrolPath,
			const float& initCost, std::vector<float> segmentsCost);

private:
	MoveBaseClient m_actionClient;
	int m_succLocs; // number of successful locations
	float m_totalDist; // total distance traveled
	std::ofstream m_logFile; // file with patrol results
	ros::Time m_startTime; // start time of current segment in patrol path
	float m_waitAtGoal; // how long (seconds) should wait at each goal
	int m_numLaps; // how many times should run patrol

	/**
	 * Sends given goal position
	 * @param xGoal x coordinate of next goal position
	 * @param yGoal y coordinate of next goal position
	 * @return True if could reach given goal, False if not
	 */
	bool sendGoal(const geometry_msgs::Point& goalPos);

	/**
	 * Updates counters and logs data regarding arrival to current patrol location
	 * @param isSuccess indicates if arrived to current patrol location successfully
	 * @param segmentCost distance traveled from previous patrol location to current one
	 */
	void handleGoal(const bool& isSuccess, const float& segmentCost);

	/**
	 * Writes given data as a new log entry
	 * @param numSuccLoc number of successive patrol locations in current round
	 * @param totalDist total distances traveled
	 * @param elapsedTime duration of logged patrol segment
	 */
	void writeLine(const int& numSuccLoc, const float& totalDist,
			ros::Duration elapsedTime);

	/**
	 * Converts given angle (degrees) to quaternion
	 * @param angle angle to convert
	 * @return quaternion matching given angle
	 */
	geometry_msgs::Quaternion angleToQuat(const float& angle);
};

#endif /* PATROLLER_H_ */
